
#include <time.h>

#include "simworld.h"
#include "SimEnvironment.h"
#include "SimRewardFunction.h"
#include "SimAction.h"
#include <string>
#include <fstream>
#include "SimAgent.h"
#include "SimSolid.h"
#include "SimSphere.h"
#include "SimBox.h"
#include <vector>

#include "Constants.h"
#include "cstate.h"
#include "caction.h"

void readSphere(std::ifstream & expt, SimSphere *sphere ){
	opal::Matrix44r transform;
	string s;
	expt>>s;
	double x,y,z,rot;
	if(s.compare(translate) == 0){
		expt >>x>>y>>z;
		sphere->transform.translate(x,y,z);
		expt>>s;
	}

	if(s.compare(rotate) == 0){
		expt >>rot>>x>>y>>z;
		sphere->transform.rotate(rot,x,y,z);
		expt>>s;
	}

	int isStat;
	if(s.compare(isStatic) == 0) {
		expt>>isStat;
		if(isStat == 0) {
			sphere->isStatic = false;
		} else {
			sphere->isStatic = true;
		}
		expt>>s;
	}

	double f;
	if(s.compare(friction) == 0){
		expt>>f;
		sphere->friction = f;
		expt>>s;
	}

	double r;
	if(s.compare(radius) == 0){
		expt>>r;
		sphere->radius = r;
	}

}

void readBox(std::ifstream & expt, SimBox *box ){
	string s;
	expt>>s;
	double x,y,z,rot;
	if(s.compare(translate) == 0){
		expt >>x>>y>>z;
		box->transform.translate(x,y,z);
		expt>>s;
	}
	if(s.compare(rotate) == 0){
		expt >>rot>>x>>y>>z;
		box->transform.rotate(rot,x,y,z);
		expt>>s;
	}

	int isStat;
	if(s.compare(isStatic) == 0) {
		expt>>isStat;
		if(isStat == 0) {
			box->isStatic = false;
		} else {
			box->isStatic = true;
		}
		expt>>s;
	}

	double f;
	if(s.compare(friction) == 0){
		expt>>f;
		box->friction = f;
		expt>>s;
	}

	if(s.compare(dimensions) == 0){
		expt>>x>>y>>z;
		box->dimensions = opal::Vec3r(x,y,z);
	}
}

void* createAgentAndLearn(void *createInfo);

struct CreateAgentInfo{
public:
	int exptNum;
	int toLearn;
	char * load_q_filename;
	SimEnvironment *simEnv;
	double epsilon;
	int episodes;
	int functionApproxType;
	int learnerType;
	pthread_t threadId;
	string loadQFileName;

	//CreateAgentInfo(){}

};


// This is the entry point for this application
int main(int argc, char **argv)
{
	 srand ( time(NULL) );
	  if(argc < 4){// && argc != 5) {
		  std::cout<<" Error:: Correct usage: LocomotionLearner.o <experiment_num> <showSim=0 or 1> <tolearn = 0> <episodeNum> "<<std::endl;
		  std::cout<<" or "<<std::endl;
		  std::cout<<" Error:: Correct usage: LocomotionLearner.o <experiment_num> <showSim=0 or 1> <tolearn = <Num Learners>> <load_q_filename>"<<std::endl;
		  return -1;
	  }

	  int exptNum = atoi(argv[1]);
	  int toShowSim = atoi(argv[2]);
	  int toLearn = atoi(argv[3]);



	  //char * load_q_filename = "nofile";

	 int numEpisode = 1;
	 if(toLearn == 0 && argc == 5){
		 numEpisode = atoi(argv[4]);
	 }
/*
	 if(toLearn >= 1 && argc >= 5){
		 for(int i=0; i< toLearn && argc >= 5 + i; i++){

			 loadQFileNames.push_back(a);
		 }
		 //load_q_filename = argv[4];
	 }*/

		std::cout<<" Experiment Num: "<<exptNum<<std::endl;
		std::cout<<" Show Simulator: "<<toShowSim<<std::endl;
		std::cout<<" To Learn : "<<toLearn<<std::endl;
		std::cout<<" Episode Num: "<<numEpisode<<std::endl;

		//std::cout<<" load_q_filename: "<<load_q_filename<<std::endl;

	  char exptNumStr[5];
	  sprintf(exptNumStr, "%d", exptNum);
	  string filename = experiment + exptNumStr + txtFile;
	  std::ifstream expt;
	  expt.open(filename.c_str());

	  Simworld *myworld = new Simworld();
	  if(toShowSim == 1){
	  	myworld->setShowWindow(true);
	  } else {
	  	myworld->setShowWindow(false);
	  }
	  myworld->init();

	  string s;
	  expt>>s;
	  int numAgents;
	  int numNonAgents;
	  Creature *c;

	  opal::Simulator *gSim = myworld->getOpalSimulator();
	  std::vector<opalSamples::Entity*>* gEntities= myworld->getEntities();
	  if(s.compare(agents) == 0) {
		  expt>>numAgents;
	  }

	  // for multiagents
	  std::vector<string> loadQFileNames;

	  for(int i=0; i< numAgents; i++){
		 if(argc >= 5 + i){
			 string a;
			 a = argv[4 + i];
			 loadQFileNames.push_back(a);
		 } else {
			 string a = nofile;
			 loadQFileNames.push_back(a);
		 }
	  }

	  for(int i=0; i< loadQFileNames.size(); i++){
		  std::cout<<" agent: "<<i<<" loadQFileName: "<<loadQFileNames[i]<<std::endl;
	  }

	  expt>>s;
	  if(s.compare(nonAgents) == 0) {
	  		  expt>>numNonAgents;
	  }
	  for(int i=0;i< numAgents;i++){

			expt>>s;
			if(s.compare(beginAgent) == 0){
		//	std::cout<<s;
				c = new Creature(gSim, gEntities );
				expt>>s;
				int numSolids;
				if(s.compare(solidsIndependent) == 0){
					expt>>numSolids;
				}
				expt>>s;
				int numJoints;
				if(s.compare(joints) == 0){
					expt>>numJoints;
				}

				for(int j=0; j< numSolids; j++) {

					expt>>s;
					if(s.compare(beginSolid) == 0){
						opal::Solid* solid =  gSim->createSolid();
						opal::Matrix44r transform;

						expt>>s;
						if(s.compare(sphere) == 0){

							SimSphere *sphere = new SimSphere(solid, gEntities);
							readSphere(expt, sphere);
							c-> create(sphere);

						} else {
							if(s.compare(box) == 0){
								SimBox *box = new SimBox(solid, gEntities);
								readBox(expt, box);
								c-> create(box);
							}
						}

						expt>>s;
						if(s.compare(endSolid) == 0){
						}
					}
				}

			}
			expt>>s;
			if(s.compare(endAgent)==0){

			}

			myworld->agents.push_back(c);

	  }

	  for(int i=0;i< numNonAgents;i++){

			expt>>s;
			if(s.compare(beginNonAgent) == 0){
		//	std::cout<<s;
				c = new Creature(gSim, gEntities );

				expt>>s;
				if(s.compare(solidsIndependent) == 0){
					int numSolids;
					expt>>numSolids;
				}
				expt>>s;
				if(s.compare(joints) == 0){
					int numJoints;
					expt>>numJoints;
				}

				expt>>s;
				if(s.compare(beginSolid) == 0){
					opal::Solid* solid =  gSim->createSolid();
					opal::Matrix44r transform;

					expt>>s;
					if(s.compare(sphere) == 0){
						SimSphere *sphere = new SimSphere(solid, gEntities);
						readSphere(expt, sphere);
						c-> create(sphere);

					} else {
						if(s.compare(box) == 0){
							SimBox *box = new SimBox(solid, gEntities);
							readBox(expt, box);
							c-> create(box);
						}
					}

					expt>>s;
					if(s.compare(endSolid) == 0){
					}
				}

			}
			expt>>s;
			if(s.compare(endNonAgent)==0){

			}

			myworld->non_agents.push_back(c);

	  }
	  double epsilon;
	  int episodes;

		expt>>s;

		int learnerType = LEARNER_SARSA;
		int rewardType = REWARD_DEFAULT;

	  if(s.compare(parameters)==0){
	  std::cout<<"Parameters:: ";
		  expt>>s;
		  if(s.compare(epsilonStr)==0){
			  expt>>epsilon;
			  std::cout<<"\t epsilon: "<<epsilon;
			  expt>>s;
		  }
		  if(s.compare(episodesStr)==0){
			  expt>>episodes;
			  std::cout<<"\t episodes: "<<episodes <<std::endl;
			  expt >> s;
		  }
		 if(s.compare(learner)==0){
			 expt>>s;
			 if(s.compare(sarsa) == 0){
				 learnerType = LEARNER_SARSA;
			 } else if(s.compare(qlearner) == 0){
				 learnerType = LEARNER_Q;
			 }
			 expt>>s;
		 }
		 if(s.compare(reward) == 0){
			 expt>>s;
			 if(s.compare(potential) == 0){
				 rewardType = REWARD_POTENTIAL;
				 std::cout<<"reward:: potential"<<std::endl;
			 }
			 expt>>s;
		 }
	  }


	  if(s.compare(humanControl) == 0){

		  std::cout<<" Human control: "<<s<<std::endl;
		  expt>>s;
	//	  std::cout<<" nonAgent: "<<s<<std::endl;
		  int humanControlAgentNum;
		  if(s.compare(nonAgent) == 0){

			  myworld->isHumanControlAgent = false;
			  expt>>myworld->humanControlledAgentNum;
			  expt>>s;
			  expt>>myworld->humanControlledSolidNum;

		  } else {
			  	if(s.compare(agent) == 0) {
			  		myworld->isHumanControlAgent = true;
			  		expt>>myworld->humanControlledAgentNum;
			  		expt>>s;
			  		expt>>myworld->humanControlledSolidNum;
			  	}
		  }
		  expt>>s;
	  }
	  int functionApproxType = FN_APPROX_TILING;

	  if(s.compare(functionApproximator) == 0){
		  std::cout<<" fun approx : "<<s;
		  expt>>s;
		  if(s.compare(tiling) == 0){
			  functionApproxType = FN_APPROX_TILING;
			  std::cout<<"\ttiling"<<std::endl;
		  } else if(s.compare(rbf) == 0){
			  functionApproxType = FN_APPROX_RBF;
			  std::cout<<"\trbf"<<std::endl;
		  } else if(s.compare(neuralNetwork) == 0){
			  functionApproxType = FN_APPROX_NEURAL;
			  std::cout<<"\tNeural Network"<<std::endl;
		  }
	  } else {
		  std::cout<<"\t default: tiling"<<std::endl;
	  }

	  bool checkWorldFormed = false;
	  if(checkWorldFormed) {
		  myworld->simulate(200);
	  	  return 0;
	  }

	  std::cout<<"Agents::"<<std::endl;
	  for(int i=0; i< myworld->agents.size(); i++){
		  Creature * c = myworld->agents[i];
		  std::cout<<" agent: " <<i<<" solids: "<<c->getNumSolids()<<std::endl;
	  }
	  std::cout<<"Non Agents::"<<std::endl;
	  for(int i=0; i< myworld->non_agents.size(); i++){
		  Creature * c = myworld->non_agents[i];
		  std::cout<<" non agent: " <<i<<" solids: "<<c->getNumSolids()<<std::endl;
	  }

	// Simulate for some time ---
	  // Important to settle things.
	myworld->simulate(20);



	SimEnvironment *simEnv;
	switch(exptNum){

		case EXPERIMENT_1_SPHERE_GOAL:
			//printf("calling SimEnvironment()\n");
			simEnv = new SimEnvironment(2, 0, myworld, exptNum);
			break;
		case EXPERIMENT_2_PUSH_BALL:
			//printf("calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(4, 0, myworld, exptNum);
			 break;

		case EXPERIMENT_3_JOINT_ACTION_PUSH:
					//printf("calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(6, 0, myworld, exptNum);
			 break;

		case EXPERIMENT_4_COOPERATION_OBSTACLE:
					//printf("calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(4, 0, myworld, exptNum);
			 break;

		case EXPERIMENT_5_CAPTURE_HUMAN:
					//printf("calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(4, 0, myworld, exptNum);
			 break;

		case EXPERIMENT_6_MULTI_AGENT_PUSH:
					//printf("calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(6, 0, myworld, exptNum);
			 break;

		case EXPERIMENT_7_JOINTS:
					//printf("calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(4, 1, myworld, exptNum);
			 break;

		case EXPERIMENT_8_MULTI_IL_PUSH:
					//printf("ADI::calling SimEnvironment()\n");
			 simEnv = new SimEnvironment(4, 0, myworld, exptNum);
			 break;

		 default:
		 break;

	}


	//std::cout<<"simenv created"<<std::endl;

	simEnv->mainThreadId = pthread_self();
	simEnv->rewardType = rewardType;

	int num_threads = numAgents;


	if(pthread_barrier_init(&(simEnv->simBarrier), NULL, num_threads))
       {
                cout<< " Could not create a Barrier " << endl;
                return -1;
        }


	pthread_t *threads = (pthread_t*)malloc(sizeof(pthread_t) * num_threads);

	std::cout<<"thread main : "<<pthread_self()<<std::endl;

	for(int i = num_threads-1 ; i>=0 ;i-- )
	{
		CreateAgentInfo *cinfo = new CreateAgentInfo();
		cinfo->episodes = episodes;
		cinfo->epsilon = epsilon;
		cinfo->exptNum = exptNum;
		cinfo->functionApproxType = functionApproxType;
		//cinfo->load_q_filename = load_q_filename;
		cinfo->simEnv = simEnv;
		cinfo->toLearn = toLearn;
		cinfo->threadId = i;
		cinfo->learnerType = learnerType;
		cinfo->loadQFileName = loadQFileNames[i];

		if( i > 0){
			int r = pthread_create(&threads[i],NULL, createAgentAndLearn, cinfo );
		} else {
			createAgentAndLearn(cinfo);
		}
	}

	for(int i=0;i<num_threads;i++)
	{
		void *status;
		if( i > 0 ) {
			int r = pthread_join(threads[i], &status);
		}
	}

   return 0;
}

void* createAgentAndLearn(void *createInfo){

	CreateAgentInfo *cinfo = (CreateAgentInfo *)createInfo;
	SimAgent *simAgent = new SimAgent(cinfo->loadQFileName, cinfo->exptNum, cinfo->simEnv);

	simAgent->id = cinfo->threadId;
	simAgent->learner = cinfo->learnerType;
	simAgent->setParams(cinfo->epsilon, cinfo->episodes);
	simAgent->functionApproxType = cinfo->functionApproxType;
	//simAgent->loadQFileName = cinfo->loadQFileName;
	simAgent->init();

	std::cout<<"simagent initialized id: "<<cinfo->threadId<<"  "<<std::endl;


	switch(cinfo->toLearn) {

	case LEARN:
		simAgent->startLearning();
		break;

	case RUN_DUMP:
		/*
		vector<int> actionList;
		std::ifstream myfile;
		string episodesFileName = experiment + exptNumStr + episodeFile ;
		myfile.open(episodesFileName.c_str());
//		std::cout<<"opening file";
		string s;

		char c;
		if(myfile.is_open()){
			while (!myfile.eof()) {

				myfile>>s;
				while(s.compare("Episode:")!= 0 ){
					if(myfile.eof()) {
						break;
					}
					myfile>>s;
				}
				//cout<<s<<endl;
				int x;
				myfile >> x;
			//	cout<<" num episode: " <<x <<endl;
				if(x == numEpisode){
					myfile >> s;
					while(s.compare("ActionList:") != 0){
						myfile>>s;
						if(myfile.eof()) {
							continue;
						}
					}
				//	cout<< s <<endl;
					int numActions;
					myfile>>numActions;
					myfile>>s;

					for(int i=0; i< numActions; i++){
						myfile >> x;
					//	cout<<" action : " << x<<endl;
						actionList.push_back(x);
					}
				}
			}
		}

		CState *curState = new CState(simEnv->getStateProperties());


		for ( int i=0; i< actionList.size(); i++) {
			int actNum = actionList[i];
			CState *newState = new CState(simEnv->getStateProperties());
			simEnv->getResetState(curState);
			CAction* act = simAgent->getAgent()->getActions()->get(actNum);

			simEnv->transitionFunction(curState, act, newState, act->getActionData() );

			//curState = newState;
			for(int j=0; j< sizeof(CState); j++){
				char *cur = (char *)curState;
				char *news = (char *)newState;
				cur[j] = news[j];
			}

		}*/
		break;

	}
}

